//
// Created by hzj on 24-3-28.
//
#include "multi_camera_cooperation/landmark_extraction_node_ros.h"


using namespace std;

LandmarkExtractionNodeROS::LandmarkExtractionNodeROS(){
    
}

void LandmarkExtractionNodeROS::init(ros::NodeHandle &nh){

//============== Read ros parameter =====================//
    nh.param<string>("cam", cam, "camA"); 
    nh.param<string>("position", position, "platform");
    nh.param<string>("landmark_shape", landmark_shape, "Square");
    nh.param<int>("ir_binary_threshold", ir_binary_threshold, 20);
    nh.param<string>("camera_config_file_path", camera_config_file_path, "/home/hezijia/noetic_ws/src/multi_camera_cooperation/config/");
    nh.param<string>("landmark_config_file_path", landmark_config_file_path, "/home/hezijia/noetic_ws/src/multi_camera_cooperation/config/");
    nh.param<string>("ir_img_topic", ir_img_topic, " ");
    
//============================= Initialize ROS topic =============================//
#ifdef IMG_COMPRESSED
    sub_ir_img = nh.subscribe(ir_img_topic + "/compressed", 1, &LandmarkExtractionNodeROS::ir_compressed_img_cb, this);
    ROS_INFO("We are using compressed image");
#else
    sub_ir_img = nh.subscribe(ir_img_topic, 1, &LandmarkExtractionNodeROS::ir_compressed_img_cb, this);
    // sub_ir_img = nh.subscribe("/ir_mono_"+cam, 1, &LandmarkExtractionNodeROS::ir_raw_img_cb, this);
    ROS_INFO("We are using raw image");
#endif
    // sub_ir_img = nh.subscribe("/ir_mono_"+cam, 1, &LandmarkExtractionNodeROS::ir_raw_img_cb, this);
#ifdef SHOW_ORIGIN_IMG
    pub_ir_img = nh.advertise<sensor_msgs::Image>("ir_mono/origin",1);
#endif
    pub_marker_pixel_raw = nh.advertise<std_msgs::Float64MultiArray>("ir_mono/marker_pixel_raw",10);
    pub_marker_pixel = nh.advertise<std_msgs::Float64MultiArray>("ir_mono/marker_pixel",10);
    pub_ir_show_img = nh.advertise<sensor_msgs::Image>("ir_mono/show",1);
    pub_ir_binary_img = nh.advertise<sensor_msgs::Image>("ir_mono/ir_binary",1);
    sub_opticalReadyFlag = nh.subscribe("/" + cam + "/single_cam_process_ros/ir_mono/opticalReadyFlag", 1, &LandmarkExtractionNodeROS::opticalReadyFlag_cb, this);
    // sub_target_center = nh.subscribe("/target_center_" + cam, 1, &LandmarkExtractionNodeROS::target_center_cb, this);
//============================= TF Initialization =============================//
    LoadCameraConfig(camera_config_file_path + cam + ".yaml");
    LoadLandmarkConfig(landmark_config_file_path);

    ROS_INFO("Publisher subscriber initialized");
    ROS_INFO("[SWARM_DETECT] Finish initialize swarm detector, wait for data...");

    cv_ptr_compressed_ir = boost::make_shared<cv_bridge::CvImage>();
    cv_ptr_raw_ir = boost::make_shared<cv_bridge::CvImage const>();
}   

//============================= ir_img receive =============================//
void LandmarkExtractionNodeROS::ir_raw_img_cb(const sensor_msgs::Image::ConstPtr &msg){
    ROS_INFO("ir_img_cb");
//  std::cout << "ir timestamp = " << msg->header.stamp << std::endl;
    stamp = msg->header.stamp;
    cv_bridge::CvImageConstPtr cv_ptr_raw_ir;
    cv_ptr_raw_ir = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::MONO8);
    ir_img = cv_ptr_raw_ir->image;
//  cv::equalizeHist(ir_img, ir_img); //对图像均衡化
#ifdef SHOW_ORIGIN_IMG
    auto ir_img_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", ir_img).toImageMsg();
    pub_ir_img.publish(ir_img_msg);
#endif
    if(ir_img.empty()){
        printf(RED"Not receive valid ir_img\n");
        return;
    }
    landmark_pose_extract();
}

void LandmarkExtractionNodeROS::ir_compressed_img_cb(const sensor_msgs::CompressedImage::ConstPtr &msg){
//    std::cout << "ir timestamp = " << msg->header.stamp << std::endl;
    stamp = msg->header.stamp;
    cv_ptr_compressed_ir = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::MONO8);
    ir_img = cv_ptr_compressed_ir->image;
//    cv::equalizeHist(ir_img, ir_img); //对图像均衡化
//    ir_img = cv::imdecode(msg->data, cv::IMREAD_GRAYSCALE);
#ifdef SHOW_ORIGIN_IMG
    auto ir_img_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", ir_img).toImageMsg();
    pub_ir_img.publish(ir_img_msg);
#endif
    // if(!yaw_initialized_flag){
    //   yaw_initialized_flag = init_relative_yaw();
    //   return;
    // }
    if(ir_img.empty()){
        printf(RED"Not receive valid ir_img\n");
        return;
    }
    landmark_pose_extract();
}

// void LandmarkExtractionNodeROS::target_center_cb(const geometry_msgs::Point::ConstPtr &msg){
//     target_center.x = msg->x;
//     target_center.y = msg->y;

//     cout << "target_center from tf: " << target_center << endl;
// }

void LandmarkExtractionNodeROS::opticalReadyFlag_cb(const std_msgs::Bool &msg){
    opticalReadyFlag = msg.data;
}
//============================= ir_img receive =============================//

void LandmarkExtractionNodeROS::landmark_pose_extract(){
    printf(BOLDREDPURPLE"[Landmark Pose Extract] Start to extract landmark pose...\n" RESET);

    ir_img_color_show = cv::Mat::zeros(ir_img.size(), CV_8UC3);
    cv::cvtColor(ir_img, ir_img_color_show, CV_GRAY2BGR);

    // Generate the trust region
    img_height = ir_img.rows;
    img_width = ir_img.cols;
    if(target_center.x == 0 && target_center.y == 0){
        target_center.x = ir_img.cols / 2;
        target_center.y = ir_img.rows / 2;
    }
    if(opticalReadyFlag){
        target_center.x = 0;
        target_center.y = 0;
        for(int i = 0; i < marker_pixels.size(); i++){
            target_center.x += marker_pixels[i].x;
            target_center.y += marker_pixels[i].y;
        }
        target_center.x /= marker_pixels.size();
        target_center.y /= marker_pixels.size();
    }else{
        target_center.x = 0;
        target_center.y = 0;
        roi_width = 4000;
        roi_height = 2000;
    }
    trust_region.x = target_center.x - roi_width / 2;
    trust_region.y = target_center.y - roi_height / 2;
    trust_region.width = roi_width;
    trust_region.height = roi_height;
    if(trust_region.width <= 0 || trust_region.width > img_width){
        trust_region.width = img_width;
    }
    if(trust_region.height <= 0 || trust_region.height > img_height){
        trust_region.height = img_height;
    }
    if(trust_region.x < 0){
        trust_region.width = roi_width - abs(trust_region.x);
        trust_region.x = 0;
    }
    if(trust_region.x + trust_region.width > img_width){
        trust_region.width = img_width - trust_region.x;
    }
    if(trust_region.x + trust_region.width < 0){
        trust_region.x = 0;
        trust_region.width = img_width;
    }
    if(trust_region.y < 0){
        trust_region.height = roi_height - abs(trust_region.y);
        trust_region.y = 0;
    }
    if(trust_region.y + trust_region.height > img_height){
        trust_region.height = img_height - trust_region.y;
    }
    if(trust_region.y + trust_region.height < 0){
        trust_region.y = 0;
        trust_region.height = img_height;
    }
    printf(GREEN"[Landmark Pose Extract] Trust_Region: x: %d, y: %d, width: %d, height: %d\n" RESET, trust_region.x, trust_region.y, trust_region.width, trust_region.height);
    
    // Prepare for the landmark extraction
    if(opticalReadyFlag){
        pre_marker_pixels = marker_pixels;
    }else{
        pre_marker_pixels.clear();
    }
    marker_pixels.clear();

    // Process the ir_img
    roiGoodFlag = ir_img_process(ir_img, marker_pixels, pre_marker_pixels, 2);

    if(roiGoodFlag){
        if(opticalGoodFlag || ShapeGoodFlag){
            roi_width = max(marker_pixels[0].x, max(marker_pixels[1].x, max(marker_pixels[2].x, marker_pixels[3].x))) - min(marker_pixels[0].x, min(marker_pixels[1].x, min(marker_pixels[2].x, marker_pixels[3].x)));
            roi_height = max(marker_pixels[0].y, max(marker_pixels[1].y, max(marker_pixels[2].y, marker_pixels[3].y))) - min(marker_pixels[0].y, min(marker_pixels[1].y, min(marker_pixels[2].y, marker_pixels[3].y)));
            roi_width *= 2;
            roi_height *= 3;

            landmark_msg_generate(landmark_msg, marker_pixels, opticalGoodFlag);
            pub_marker_pixel.publish(landmark_msg);

            // print the marker pixels
            for (int i = 0; i < marker_pixels.size(); i++) {
                ROS_INFO("pointsVector-> x:%f, y:%f", marker_pixels[i].x, marker_pixels[i].y);
            }
        }else{
            opticalReadyFlag = false;
        }
    

        #ifdef ENABLE_VISUALIZATION    
            if(opticalGoodFlag || ShapeGoodFlag){
                for (int i = 0; i < marker_pixels.size(); i++) { 
                    cv::circle(ir_img_color_show, cv::Point2i(marker_pixels[i].x, marker_pixels[i].y), 4,  cv::Scalar(0,0,255), 1, cv::LINE_AA);
                    cv::putText(ir_img_color_show, std::to_string(i), cv::Point2i(marker_pixels[i].x, marker_pixels[i].y), cv::FONT_HERSHEY_TRIPLEX, 0.65, cv::Scalar(0,255,0), 1, false);
                }
                cv::rectangle(ir_img_color_show, trust_region, cv::Scalar(255, 0, 0), 1);
            }else{
                printf(RED"[Shape identify process] Shape not good!\n" RESET);
            }
            
            auto ir_show_msg_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", ir_img_color_show).toImageMsg();
            pub_ir_show_img.publish(ir_show_msg_img);
        #endif
    }else{
        printf(RED"[ROI Process] ROI NOT GOOD!\n" RESET);
    }
}


bool LandmarkExtractionNodeROS::ir_img_process(cv::Mat &_ir_img, vector<cv::Point2f> &pointsVector, vector<cv::Point2f> &pre_pointsVector, int model){
    printf(BOLDCYAN"[Image Process] IR Image Process Start.\n" RESET);
    if(_ir_img.empty())
    {
        ROS_ERROR("ir_img_process, NO _ir_img !!!");
        return false;
    }

    cv::Mat trust_region_mask = cv::Mat::zeros(img_height, img_width, CV_8UC1);
    trust_region_mask.setTo(0);
    ir_img_crop.setTo(0);
    trust_region_mask(trust_region).setTo(255);
    cv::Mat trust_ir_img = _ir_img;
    trust_ir_img.copyTo(ir_img_crop, trust_region_mask);

    if(model == 1){//尝试用FAST检测特征点
        if(extractFeatures(ir_img_crop, pointsVector)){
            return true;
        }else{
            return false;
        }
    }

    if(model == 2){//尝试用binary检测特征点
        if(binary_threshold(ir_img_crop, pointsVector, pre_pointsVector)){
            return true;
        }else{
            return false;
        }
    }

    ROS_ERROR("error process method, ir_img_process failed !!!");
    return false;

}

bool LandmarkExtractionNodeROS::extractFeatures(cv::Mat &frame, vector<cv::Point2f> &pointsVector){
    pointsVector.clear();
    cv::goodFeaturesToTrack(frame, pointsVector, landmark_num, 0.01, 10);
    if(pointsVector.size() != 0){
        return true;
    }else{
        return false;
    }
}

bool LandmarkExtractionNodeROS::binary_threshold(cv::Mat &frame, vector<cv::Point2f> &pointsVector, vector<cv::Point2f> &pre_pointsVector){
    pointsVector.clear();

    int try_count = 0;

    while(1){
        //二值化及膨胀腐蚀
        threshold(frame, ir_binary, ir_binary_threshold, 255, cv::THRESH_BINARY);

        if(position == "platform"){
            ir_erode = ir_binary;
        }else if(position == "uav"){
            cv::erode(ir_binary, ir_erode, erodeElement);
        }
        
        
        //寻找轮廓
        findContours(ir_erode, ir_contours_unfiltered, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

        // filter the contours
        ir_contours.clear();
        for (int i = 0; i < ir_contours_unfiltered.size(); i++){
                double area = cv::contourArea(ir_contours_unfiltered[i]);
                // printf(WHITE "Contour Area: %f\n", area);
                if((area < max_area_threshold) && (area >= min_area_threshold)){
                    ir_contours.push_back(ir_contours_unfiltered[i]);
                }
                // ir_contours.push_back(ir_contours_unfiltered[i]);
        }
        // cout << "ir_contours size: " << ir_contours.size() << endl;
        ir_contours_unfiltered.clear();
        
        if(ir_contours.size() < landmark_num){
            ir_binary_threshold -= 5;
            printf(YELLOW "[down] find %zu, ir_binary_threshold set to %d\n" RESET, ir_contours.size(), ir_binary_threshold);
            if(ir_binary_threshold <= 20){
                ir_binary_threshold = 80;
                break;
            }
        }else if(ir_contours.size() > landmark_num * 2){
            ir_binary_threshold += 5;
            printf(YELLOW "[up] find %zu, ir_binary_threshold set to %d\n" RESET, ir_contours.size(), ir_binary_threshold);
            if(ir_binary_threshold >= 220){
                ir_binary_threshold = 80;
                break;
            }
        }else{
            printf(GREEN"find %d, ir_binary_threshold set to %d\n" RESET , landmark_num, ir_binary_threshold);
            break;
        }

        if(try_count++ > 30){ //如果30次都没有找到合适阈值，就放弃
            ir_binary_threshold = 80;
            break;
        }
    }

    //在ROI图像上画出边框,可视化发布边框
    cv::Mat ir_binary_color_show;
    cv::cvtColor(ir_erode, ir_binary_color_show, CV_GRAY2BGR);
    cv::rectangle(ir_binary_color_show, trust_region, cv::Scalar(255, 0, 0), 1);

    auto ir_binary_msg_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", ir_binary_color_show).toImageMsg();
    pub_ir_binary_img.publish(ir_binary_msg_img);


    if (ir_contours.size() > 0){
        for (int i = 0; i < ir_contours.size(); i++) {
            cv::Rect bbox;
            bbox = boundingRect(ir_contours[i]);
            ir_contours_raw.emplace_back(cv::Point2f((bbox.tl() + bbox.br()).x / 2.0, (bbox.tl() + bbox.br()).y / 2.0));
        }
    }

    // // 再采用灰度图去refine坐标
    // if(!refine_pixel(ir_contours_final, pointsVector, ir_img)){
    //     printf(RED"refine_pixel failed! use origin points.\n" RESET);
    //     pointsVector.clear();
    //     pointsVector = ir_contours_final;
    // }

    // test
    // delete too close points
    for(int i = 0; i < ir_contours_raw.size(); i++){
        for(int j = i + 1; j < ir_contours_raw.size(); j++){
            if(distanceBetweenPoints(ir_contours_raw[i], ir_contours_raw[j]) < 5){
                ir_contours_raw.erase(ir_contours_raw.begin() + j);
                j--;
            }
        }
    }

    // // Publish the marker pixel raw
    landmark_msg_generate(landmark_msg_raw, ir_contours_raw, true);
    pub_marker_pixel_raw.publish(landmark_msg_raw);

    pointsVector = ir_contours_raw;

    ir_contours.clear();
    ir_contours_raw.clear();
    ir_contours_final.clear();

    if(pointsVector.size() != 0){
        // opticalReadyFlag = false;
        // opticalGoodFlag = false;

         //csj0122
        // if(opticalReadyFlag && pre_pointsVector.size() != 0){
        //     opticalGoodFlag = optical_flow(pointsVector, pre_pointsVector); //csj0122
        // }else{
        //     opticalGoodFlag = false;
        // }

        // if(opticalGoodFlag){
        //     return true;
        // }else{
            pre_pointsVector.clear();
            opticalReadyFlag = false;
            
            #ifdef USE_4_Point
                if(pointsVector.size() > (4 - 1)){
                    ShapeGoodFlag = findBestFourPoints(pointsVector);  
                }else{
                    ShapeGoodFlag = false;
                }      
            #endif

            return true;
        // }

    }else{
        pre_pointsVector.clear();
        opticalReadyFlag = false;
        return false;
    }
}

/**
 * @brief 从不同范围尺度和阈值计算marker_pixel的中心点
 * @param pts_raw
 * @return
 */
bool LandmarkExtractionNodeROS::refine_pixel(std::vector<cv::Point2f> &pts_raw, std::vector<cv::Point2f> &pts_refine, cv::Mat &img){
//    cv::HoughCircles
    for(int i = 0; i < pts_raw.size(); i++){
        cv::rectangle(ir_img_color_show, cv::Point2f(pts_raw[i].x-win_width, pts_raw[i].y-win_width),
                      cv::Point2f(pts_raw[i].x+win_width, pts_raw[i].y+win_width),cv::Scalar(200,20,0),1);

        const int n = (2*win_width+1) * (2*win_width+1); //窗内最多这么多像素值会被加入计算
        Eigen::Matrix3Xi filterPts; // xy存储像素位置，z存储权重 3行n列
        filterPts.resize(3, n);
        int count_filter = 0;
        int weight_sum = 0; //权重总数
        int threshold = 200; //选取大于阈值的点用作中心计算
        //取均值思路，但为每个超过阈值而参与的点，根据灰度值赋予融合权重
        //若threshold初始取的不合适，则至少选出一个点再跳出
        while(1){
//            printf(GREEN "threshold = %d\n" RESET, threshold);
            filterPts.setZero();
            weight_sum = 0;
            count_filter = 0;
            for(int j = -win_width + 1; j < win_width; j++){
                for(int k = -win_width + 1; k < win_width; k++){
                    int index_x = j + pts_raw[i].x;
                    int index_y = k + pts_raw[i].y;
                    if(img.at<uchar>(index_y,index_x) > threshold){ //把阈值调整，让寻找中心更准确
                        filterPts(0, count_filter) = index_x;
                        filterPts(1, count_filter) = index_y;
                        filterPts(2, count_filter) = img.at<uchar>(index_y,index_x) - threshold; //xy位置，和超过阈值的灰度值(作为权重 )
                        weight_sum += filterPts(2, count_filter);
                        count_filter++;
                    }
                }
            }
//            printf(REDPURPLE "refine threshold now = %d\n" RESET, threshold);
            if(count_filter < 2){ //最少也要选出2个点来做权重融合，如果是1个或者0个就用之前的pts_raw
                threshold -= 1; //若阈值太高选不出点，则降低阈值
                if(threshold < 100){
                    printf(REDPURPLE "refine threshold too low = %d, return false ..., maybe optical flow track failed\n" RESET, threshold);
                    return false;
                }
            }else if(count_filter > 9){
                threshold += 1; //若阈值太低选出点过多，则增加阈值
                if(threshold == 255){
                    printf(REDPURPLE "refine threshold too high = %d, use all the points\n" RESET, threshold);
                    break;
                }
            }else{
//                printf(GREEN "refine count_filter = %d\n" RESET, count_filter);
                break;//选取数目合适，跳出循环
            }
        }
//        printf(GREEN "count_filter: %d, weight_sum: %d\n" RESET, count_filter, weight_sum);
        float u_pixel_weighted = 0;
        float v_pixel_weighted = 0;
        for (int j = 0; j < count_filter; ++j) {
            u_pixel_weighted += filterPts(0,j) * 1.0*filterPts(2,j)/weight_sum; //u pixel 乘上该像素权重在总权重中的占比
            v_pixel_weighted += filterPts(1,j) * 1.0*filterPts(2,j)/weight_sum; //v pixel 乘上该像素权重在总权重中的占比
//            printf(YELLOW "u: %d, v:%d, weight:%f\n" RESET,filterPts(0,j), filterPts(1,j), 1.0*filterPts(2,j)/weight_sum);
        }
        pts_raw[i].x = u_pixel_weighted;
        pts_raw[i].y = v_pixel_weighted;
//        printf(REDPURPLE "pts[%zu] u_pixel_weighted: %f, v_pixel_weighted: %f\n" RESET,i, u_pixel_weighted, v_pixel_weighted);
        pts_refine.emplace_back(pts_raw[i]);
    }
    return true;
}

void LandmarkExtractionNodeROS::landmark_msg_generate(std_msgs::Float64MultiArray &msg, vector<cv::Point2f> &pointsVector, bool opticalGoodFlag){  
    msg.data.clear();
    
    msg.layout.dim.push_back(std_msgs::MultiArrayDimension());
    msg.layout.dim[0].size = pointsVector.size();
    msg.layout.dim[0].stride = 1;
    msg.layout.dim[0].label = "points";

    if(opticalGoodFlag){
        msg.data.push_back(1);
    }else{
        msg.data.push_back(0);
    }
    for (int i = 0; i < pointsVector.size(); i++)  
    {  
        msg.data.push_back(pointsVector[i].x);
        msg.data.push_back(pointsVector[i].y);      
    }
}

bool LandmarkExtractionNodeROS::Square_shape_identity(vector<cv::Point2f> &pointsVector){

    marker_pixels_sorted.clear();
    marker_pixels_up.clear();
    marker_pixels_down.clear();
    
    if (pointsVector.size() != 4)
    {
        return false;
    }

    // we choose to sort the points by their x coordinate first, then by their y coordinate
    sort(pointsVector.begin(), pointsVector.end(), [](const cv::Point2f& pt1, const cv::Point2f& pt2) {
        return pt1.x < pt2.x;
    });

    std::vector<cv::Point2f> marker_pixels_left(pointsVector.begin(), pointsVector.begin() + 2);
    std::vector<cv::Point2f> marker_pixels_right(pointsVector.begin() + 2, pointsVector.end());

    sort(marker_pixels_left.begin(), marker_pixels_left.end(), [](const cv::Point2f& pt1, const cv::Point2f& pt2) {
        return pt1.y < pt2.y;
    });

    sort(marker_pixels_right.begin(), marker_pixels_right.end(), [](const cv::Point2f& pt1, const cv::Point2f& pt2) {
        return pt1.y < pt2.y;
    });

    marker_pixels_sorted.push_back(marker_pixels_right[0]); // right up
    marker_pixels_sorted.push_back(marker_pixels_right[1]); // right down
    marker_pixels_sorted.push_back(marker_pixels_left[1]); // left down
    marker_pixels_sorted.push_back(marker_pixels_left[0]); // left up

    // Check the shape of the marker
    Eigen::Vector2d linkvector1 = subtractPoints(marker_pixels_sorted[3], marker_pixels_sorted[0]); // up line
    Eigen::Vector2d linkvector2 = subtractPoints(marker_pixels_sorted[2], marker_pixels_sorted[1]); // down line
    Eigen::Vector2d linkvector3 = subtractPoints(marker_pixels_sorted[3], marker_pixels_sorted[2]); // left line
    Eigen::Vector2d linkvector4 = subtractPoints(marker_pixels_sorted[0], marker_pixels_sorted[1]); // right line

    double angle1 = abs(vectorAngle(linkvector1, linkvector2, 1));
    double angle2 = abs(vectorAngle(linkvector3, linkvector4, 1));

    if(angle1 > 20){
        return false;
    }else{
        shape_score += min(2.0, 10 / angle1);
    }
    if(angle2 > 90){
        return false;
    }else{
        shape_score += min(2.0, 20 / angle2);
    }
    
    // for(int i = 0; i < 4; i++){
    //     for(int j = i + 1; j < 4; j++){
    //         double distance = distanceBetweenPoints(marker_pixels_sorted[i], marker_pixels_sorted[j]);
    //         if(distance < 50){
    //             return false;
    //         }
    //     }
    // }

    pointsVector.clear();
    pointsVector = marker_pixels_sorted;

    return true;
    

}

bool LandmarkExtractionNodeROS::T_shape_identify(vector<cv::Point2f> &pointsVector){
    marker_pixels_sorted.clear();
    marker_pixels_up.clear();
    marker_pixels_down.clear();
    min_delta_angle = 180;
    parrallel_possibility = {};

    for(int i = 0; i < 4; i++){
        FeaturesSort(pointsVector[i], pointsVector[(i + 1) % 4], pointsVector[(i + 2) % 4], pointsVector[(i + 3) % 4]);
        FeaturesSort(pointsVector[i], pointsVector[(i + 2) % 4], pointsVector[(i + 3) % 4], pointsVector[(i + 1) % 4]);
        FeaturesSort(pointsVector[i], pointsVector[(i + 3) % 4], pointsVector[(i + 1) % 4], pointsVector[(i + 2) % 4]);
    }

    if(parrallel_possibility.size() == 0){
        // cout << "parrallel_possibility.size() == 0" << endl;
        return false;
    }else{
        double min_delta_arm = numeric_limits<double>::max();
        for(int i = 0; i < parrallel_possibility.size(); i++){
            double delta_arm = abs(distanceBetweenPoints(parrallel_possibility[i][0], parrallel_possibility[i][2]) - distanceBetweenPoints(parrallel_possibility[i][0], parrallel_possibility[i][3]));
            if(delta_arm < min_delta_arm){
                min_delta_arm = delta_arm;
                marker_pixels_up.clear();
                marker_pixels_down.clear();
                marker_pixels_up.emplace_back(parrallel_possibility[i][0]);
                marker_pixels_up.emplace_back(parrallel_possibility[i][2]);
                marker_pixels_up.emplace_back(parrallel_possibility[i][3]);
                marker_pixels_down.emplace_back(parrallel_possibility[i][1]);
            }
        }
    }

    if(marker_pixels_up.size() != 3 || marker_pixels_down.size() != 1){
        // cout << "marker_pixels_up.size() != 3 || marker_pixels_down.size() != 1" << endl;
        return false;
    }

    marker_pixels_sorted.emplace_back(marker_pixels_up[0]);

    Eigen::Vector2d linkvector =  subtractPoints(marker_pixels_up[0], marker_pixels_down[0]);
    Eigen::Vector2d linkvector0 = subtractPoints(marker_pixels_up[0], marker_pixels_up[1]);
    Eigen::Vector2d linkvector1 = subtractPoints(marker_pixels_up[0], marker_pixels_up[2]);

    if(checkRotationDirection(linkvector, linkvector0)){
        marker_pixels_sorted.emplace_back(marker_pixels_up[1]);
        marker_pixels_sorted.emplace_back(marker_pixels_up[2]);
    }else if(checkRotationDirection(linkvector, linkvector1)){
        marker_pixels_sorted.emplace_back(marker_pixels_up[2]);
        marker_pixels_sorted.emplace_back(marker_pixels_up[1]);
    }

    marker_pixels_sorted.emplace_back(marker_pixels_down[0]);
    
    pointsVector.clear();
    pointsVector = marker_pixels_sorted;  

    if(pointsVector.size() != landmark_num){
        // cout << "pointsVector.size() != landmark_num" << endl;
        return false;
    } 

    // Check the shape of the marker
    Eigen::Vector2d vec10 = subtractPoints(pointsVector[1], pointsVector[0]);
    Eigen::Vector2d vec20 = subtractPoints(pointsVector[2], pointsVector[0]);
    if((abs((vectorNorm2D(vec10) - vectorNorm2D(vec20)) / (vectorNorm2D(vec10) + vectorNorm2D(vec20)))) > 0.3){
        // cout << "abs(vectorNorm2D(vec10) - vectorNorm2D(vec20)) too large" << endl;
        return false;
    }else{
        shape_score += min(2.0, 0.1 / (abs((vectorNorm2D(vec10) - vectorNorm2D(vec20)) / (vectorNorm2D(vec10) + vectorNorm2D(vec20)))));
    }

    double distance_01 = distanceBetweenPoints(pointsVector[0], pointsVector[1]);
    double distance_02 = distanceBetweenPoints(pointsVector[0], pointsVector[2]);
    double distance_03 = distanceBetweenPoints(pointsVector[0], pointsVector[3]);
    double distance_04 = distanceBetweenPoints(pointsVector[1], pointsVector[3]);
    double distance_05 = distanceBetweenPoints(pointsVector[2], pointsVector[3]);

    if(((distance_01 + distance_02 + distance_03) > 250) || (max(distance_01, max(distance_02, distance_03)) > 100) || (min(distance_01, min(distance_02, min(distance_03, min(distance_04, distance_05)))) < 10)){
        return false;
    }else{
        // cout << (distance_01 + distance_02 + distance_03) << endl;
        shape_score += min(2.0, 200 / (distance_01 + distance_02 + distance_03));
    }

    // if(pointsVector[3].y > max(pointsVector[0].y, max(pointsVector[1].y, pointsVector[2].y))){
    //     shape_score += 4;
    // }

    return true;
}

void LandmarkExtractionNodeROS::FeaturesSort(cv::Point2f &up_center, cv::Point2f &down, cv::Point2f &up_side_1, cv::Point2f &up_side_2){
    Eigen::Vector2d linkvector1 = subtractPoints(up_center, up_side_1);
    Eigen::Vector2d linkvector2 = subtractPoints(up_center, up_side_2);

    double angle = abs(vectorAngle(linkvector1, linkvector2, 1) - 180);

    if(angle < 10.0){
    // if((angle < 10.0) && (down.y > max(up_center.y, max(up_side_1.y, up_side_2.y)))){
        parrallel_possibility.emplace_back(vector<cv::Point2f>{up_center, down, up_side_1, up_side_2});
    }
    
    // if(angle < min_delta_angle){
    //     min_delta_angle = angle;

    //     marker_pixels_up.clear();
    //     marker_pixels_down.clear();

    //     marker_pixels_up.emplace_back(up_center);
    //     marker_pixels_up.emplace_back(up_side_1);
    //     marker_pixels_up.emplace_back(up_side_2);
    //     marker_pixels_down.emplace_back(down);
    // }

}

bool LandmarkExtractionNodeROS::optical_flow(vector<cv::Point2f> &pointsVector, vector<cv::Point2f> &pre_pointsVector){
    printf(BOLDCYAN"[Optical Flow] Optical Flow Start.\n" RESET);

    int bad_status_num = 0;
    int bad_status_index = landmark_num;
    vector<cv::Point2f> pointsVector_temp = {};

    for(int i = 0; i < pre_pointsVector.size(); i++){
        double min_distance = numeric_limits<double>::max();
        double min_distance_index = landmark_num;

        for(int j = 0; j < pointsVector.size(); j++){
            double distance = distanceBetweenPoints(pre_pointsVector[i], pointsVector[j]);

            if(distance < min_distance){
                min_distance = distance;
                min_distance_index = j;
            }
        }

        if(min_distance < 25){
            pointsVector_temp.emplace_back(pointsVector[min_distance_index]);
        }else{
            bad_status_num++;
            bad_status_index = i;

            if(bad_status_num > 1){
                return false;
            }else{
                pointsVector_temp.emplace_back(cv::Point2f(0, 0));
            }
        }
    }

    if(bad_status_num == 1){
        if(bad_status_index == 0){
            pointsVector_temp[0] = (pointsVector_temp[1] + pointsVector_temp[2]) / 2;
        }else if(bad_status_index == 1){
            pointsVector_temp[1] = (2 * pointsVector_temp[0] - pointsVector_temp[2]);
        }else if(bad_status_index == 2){
            pointsVector_temp[2] = (2 * pointsVector_temp[0] - pointsVector_temp[1]);
        }else if(bad_status_index == 3){
            cout << "bad_status_index == 3" << endl;
            return false;
        }
    }

    // Geometry check
    Eigen::Vector2d linkvector1 = subtractPoints(pointsVector_temp[0], pointsVector_temp[1]);
    Eigen::Vector2d linkvector2 = subtractPoints(pointsVector_temp[0], pointsVector_temp[2]);
    double angle = abs(vectorAngle(linkvector1, linkvector2, 1) - 180);
    if(angle > 10){
        cout << "angle > 10 DDDD" << endl;
        return false;
    }

    pointsVector.clear();
    pointsVector = pointsVector_temp;

    return true;
}

bool LandmarkExtractionNodeROS::findBestFourPoints(vector<cv::Point2f> &pointsVector){
    int feature_num = pointsVector.size();
    double highest_score = 0;
    ShapeGoodFlag = false;
    vector<cv::Point2f> best_points = {};
 
    // 暴力搜索所有点的组合
    for (int i = 0; i < feature_num; ++i) {
        for (int j = i + 1; j < feature_num; ++j) {
            for (int k = j + 1; k < feature_num; ++k) {
                for (int l = k + 1; l < feature_num; ++l) {
                    vector<cv::Point2f> four_points = {pointsVector[i], pointsVector[j], pointsVector[k], pointsVector[l]};
                    shape_score = 1;
                    if(landmark_shape == "Square"){
                        // printf(GREEN"[Find Best Four Points] Square Shape Identify Start.\n" RESET);
                        ShapeGoodFlag = Square_shape_identity(four_points);
                    }else if(landmark_shape == "T"){
                        // printf(GREEN"[Find Best Four Points] T Shape Identify Start.\n" RESET);
                        ShapeGoodFlag = T_shape_identify(four_points);
                    }

                    if(shape_score * ShapeGoodFlag > highest_score){
                        highest_score = shape_score * ShapeGoodFlag;
                        best_points = four_points;
                    }
                }
            }
        }
    }
    
    printf(GREEN"[Find Best Four Points] Highest Score: %f\n", highest_score);

    pointsVector.clear();
    
    if(highest_score > 1){
        pointsVector = best_points;
        return true;
    }else{
        return false;
    }
}

void LandmarkExtractionNodeROS::LoadCameraConfig(const std::string& config_path){
    std::ifstream fin(config_path);
    YAML::Node camera_config = YAML::Load(fin);
    
    auto IR_Camera = camera_config["Camera"];

    img_width = IR_Camera["image_width"].as<double>();
    img_height = IR_Camera["image_height"].as<double>();
}

void LandmarkExtractionNodeROS::LoadLandmarkConfig(const std::string& config_path){
    std::ifstream fin(config_path);
    YAML::Node landmark_config = YAML::Load(fin);

    auto IR_landmark = landmark_config["Landmark"];

    landmark_num = IR_landmark["number"].as<int>();

}